1. Field of the Invention
The present invention pertains to the filtering of sensed signals and, in particular, to a Kalman filter including an adaptive measurement variance estimator.
2. Description of the Related Art
Discrete Kalman filtering is extensively used in modem day digital control and signal processing for a variety of purposes. Among these purposes are to: “smooth” signals, i.e., reduce measurement noise; observe unmeasured states; and/or predict future system states. In a typical discrete Kalman filter, all of the system parameters are specified. These system parameters typically include the:                state transition matrix;        state input gains;        plant uncertainty covariance function matrix; and        measurement covariance function matrix.However, the measurement covariance function matrix is frequently unknown. In these cases, an ad hoc value for the measurement covariance function matrix is used. Although this estimate is better than nothing, it nevertheless impairs the performance of the Kalman filter in its intended function, i e., smoothing, observing, and/or predicting.        
For instance, consider a system described mathematically as follows using the notations defined in Table 1 below:x[n]=A[n]x[n−1]+B[n]w[n]+C[n]u[n]  (1)z[n]=H[n]x[n]+v[n]  (2)wherein:                u[n]≡the input to the system;        z[n]≡the measured output of the system; and        x[n]≡state of the system.The quantities w[n] and v[n] are mutually independent, white, noise processes with discrete variance functions Q[n]δ[n] and R[n]δ[n], respectively. In the art, Eq. (1) is known as the “state transition model” and Eq. (2) is the “measurement model.” One known filtering technique based on a Kalman filter employs the following set of equations for determining the measurement variance estimate {circumflex over (R)}[n] for the nth sample:{circumflex over (x)}[n|n−1]=A[n]{circumflex over (x)}[n−1]+C[n]u[n]  (3)P[n|n−1]=A[n]P[n−1]A[n]T+B[n]Q[n]B[n]T   (4)e[n]=z[n]−H[n]{circumflex over (x)}[n|n−1]  (5){circumflex over (σ)}e2[n]={circumflex over (σ)}e2[n−1]+H[n]G[n](e2[n]−{circumflex over (σ)}e2[n−1])   (6){circumflex over (R)}[n]=max(0, {circumflex over (σ)}e2[n]−H[n]P[n|n−1]H[n]T)   (7)        
                              K          ⁡                      [            n            ]                          =                                            P              ⁡                              [                                  n                  |                                      n                    -                    1                                                  ]                                      ⁢                          H              T                                                          H              ⁢                                                          ⁢                              P                ⁡                                  [                                      n                    |                                          n                      -                      1                                                        ]                                            ⁢                              H                T                                      +                                          R                ^                            ⁡                              [                n                ]                                                                        (        8        )            {circumflex over (x)}[n]={circumflex over (x)}[n|n−1]+K[n](z[n]−H[n]{circumflex over (x)}[n|n−1])  (9)P[n]=(I−K[n]H[n])P[n|n−1])   (10)Note the calculation of the variance estimation {circumflex over (R)}[n]. Note also that these are but mathematical descriptions of sensed signals, e.g., electrical, magnetic, or optical signals representing tangible real world phenomena or characteristics of such phenomena.
This technique can essentially be broken down into three parts. First, the current state is predicted (Eq. (3), Eq. (4)). Next, the variance of the measured quantity is estimated (Eq. (5), Eq. (6), and Eq. (7)). Then, the current state is updated with the measurement (Eq. (8), Eq. (9), Eq. (10)). This known technique's variance estimate {circumflex over (R)}[n] is often quite large. Also, it is possible for the measurement variance estimate to be zero, which happens in practice, and causes all manner of problems.
TABLE 1Definitions of Mathematical NotationSymbolDimensionsDescriptionx[n]N × 1System state vector (scalar in the case N = 1){circumflex over (x)}[n]N × 1Current state vector estimate{circumflex over (x)}[n|n − 1]N × 1Predicted state vector (estimate of state vectorfrom previouslyestimated state vector)P[n]N × NState vector estimate covariance matrixP[n|n − 1]N × NState vector prediction covariance matrixw[n]L × 1System driving noiseQ[n]L × LSystem driving noise covariance matrixu[n]J × 1System input vectorz[n]1 × 1System measurementK[n]N × 1Kalman filter gain vectorv[n]1 × 1Measurement noiseR[n]1 × 1Measurement variance{circumflex over (R)}[n]1 × 1Estimated measurement varianceA[n]N × NState transition matrixB[n]N × LSystem driving noise gain matrixC[n]N × JSystem input gain matrixH[n]1 × NInput state gain vectorG[n]1 × 1Error filter gainδ[n]1 × 1Discrete kronecker delta functione[n]1 × 1Prediction error{circumflex over (σ)}e2[n]1 × 1Prediction Error variance estimate
The present invention is directed to resolving, or at least reducing, one or all of the problems mentioned above.